% test_force_control

clc
clear
close all
format compact
addpath('../animations', '../controllers', '../dynamics', '../environments', '../library');

% x_dot_vec = [0:0.1:2.0
% 
% for iter = 1:length(x_dot_vec)

x_dot = 0;
y = 1.0;

uniform_damping = 100e3;

first_guess = (4*pi/8);

td_angle = fc_fixed_point_solve;

[time_passive, force_passive, output] = passive_force_profile;

trigger_velocity = min(output{1,2}(:,ForceControlModelClass.dyci));

y = 1.01;

[time_fc, force_fc, output_fc, J_fc, controller_fc] = run_force_control(time_passive, force_passive);

% J_opt
% J_fc

% close all
% plot(time_passive, force_passive, 'k--', time_opt, force_opt, 'r-', time_fc, force_fc, 'b-')

close all
plot(time_passive, force_passive, 'k--', time_fc, force_fc, 'b-')

figure
plot(output_fc{1,1}(:,1), output_fc{1,2}(:,4))

% dxout_fc = output_fc{3,2}(end,ForceControlModelClass.dxci)
% yout_fc = output_fc{3,2}(end,ForceControlModelClass.yci)
% 
dxout_fc_error = output_fc{3,2}(end,ForceControlModelClass.dxci) - x_dot
yout_fc_error = output_fc{3,2}(end,ForceControlModelClass.yci) - y 

% 
% max_torque = controller_fc.max_torque
% 
% T_in = 0.5*ForceControlModelClass.m*x_dot^2
% V_in = ForceControlModelClass.m*ForceControlModelClass.g*y
% 
% T_out = 0.5*ForceControlModelClass.m*dxout_fc^2
% V_out = ForceControlModelClass.m*ForceControlModelClass.g*yout_fc
% 
% E_net = T_out + V_out - T_in - V_in
% 
% E_adjust = J_fc-E_net



[time, force, output, J, controller] = run_force_control_extend(time_passive, force_passive, trigger_velocity);

close all
plot(time_passive, force_passive, 'k--', time, force, 'b-')

dxout_fc_error = output{3,2}(end,ForceControlModelClass.dxci) - x_dot
yout_fc_error = output{3,2}(end,ForceControlModelClass.yci) - y 
